// Have include guard.
#ifndef JOYSTICKDRIVECONTROLLER_H
#define JOYSTICKDRIVECONTROLLER_H

// Include files.
#include <vector>
#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "../Values/JoystickValue.hpp"
#include "../Filters/CompoundFilter.hpp"
#include "Service.hpp"

/**
 * The JoystickDriveController drives using input from a single joysticks.
 * 
 * @tparam Drive Needs to have a function called Stop that takes no arguments.
 * @tparam Handler
 */
template
<
	typename Drive,
	typename Value,
	void(*Handler)(Drive &drive, const Value &value)
>
class JoystickDriveService :
	public Service
{
	protected:
		typedef JoystickValue Input;
		typedef Value Output;
		typedef Filter<Input, Output> Filter;
		
		Drive &m_drive;
		CompoundFilter<Input, Output> m_filter;
		GenericHID &m_joystick;
	public:
		JoystickDriveService(Drive &drive, boost::shared_ptr<Filter> driveMode, GenericHID &joystick) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode),
			m_joystick(joystick)
		{
		}
		
		JoystickDriveService(Drive &drive, Filter *driveMode, GenericHID &joystick, bool canDeleteDriveMode = false) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode, canDeleteDriveMode),
			m_joystick(joystick)
		{
		}
		
		template<typename T>
		void AddFilter(T *filter, bool canDeleteFilter)
		{
			m_filter.AddFilter(filter, canDeleteFilter);
		}
		
		template<typename T>
		void AddFilter(T filter)
		{
			m_filter.AddFilter(filter);
		}
	protected:
		virtual void HandlePeriodic()
		{			
			Input input(m_joystick);
			Output output = m_filter.Apply(input);
			Handler(m_drive, output);
		}
		
		virtual void HandleStop()
		{
			m_drive.Stop();
		}
};

#endif // #ifndef JOYSTICKDRIVECONTROLLER_H 
